#pragma once

#include <iostream>
#include <vector>
#include <memory>
#include <boost/circular_buffer.hpp>
#include "common_lib/libtime.h"

enum eBrakeCmd
{
    BRAKE = 0,
    UNBRAKE = 1
};

enum eRunningType
{
    HEADFOLLOWING = 0,
    ROTATING,
    STEERTURNING
};

enum eMoveMode
{
    STILL = 0,
    TOWARD,
    TURN,
    ROTATE,
    HEADFOLLOW
};

enum eMANUALORAUTO
{
    MANUAL,
    AUTO,
    REMOTER
};

struct SteerCmd
{
    float steer_speed;
    float steer_angle;
    bool  steer_init;
    eBrakeCmd brake_cmd;
    SteerCmd()
    {
        steer_speed = 0.0f;
        steer_angle = 0.0f;
        steer_init  = false;
        brake_cmd = eBrakeCmd::BRAKE;
    }
};

struct SteerState
{
    float steer_speed;
    float steer_angle;
    uint32_t steer_state;
    SteerState()
    {
        steer_speed = 0.0f;
        steer_angle = 0.0f;
        steer_state = 0x00;
    }
};

struct SteerParams
{
    float wheel_radius;
    float wheel_ratio;
    float steer_ratio;
    int8_t wheel_reverse; // 轮子是否反向
    int8_t steer_reverse; // 转向电机是否反向
    float pos_x;
    float pos_y;
    float steer_angle_offset;

    SteerParams()
    {
        wheel_reverse = 1;
        steer_reverse = 1;
        wheel_ratio = 1.0f;
        steer_ratio = 1.0f;
        steer_angle_offset = 0.0f;
    }
};

struct AMRReceiveState
{
    eMANUALORAUTO manual_auto;
    std::vector<struct SteerState> steer_state;
    std::vector<SteerParams> steer_param;
};

struct AMRSendCmd
{
    eMoveMode move_mode;
    std::vector<struct SteerCmd> steer_cmd;
};

struct AMRCmdAndState
{
    eMoveMode move_mode;
    eMANUALORAUTO manual_auto;
    std::vector<struct SteerCmd> steer_cmd;
    std::vector<struct SteerState> steer_state;
    std::vector<SteerParams> steer_param;
};

// struct AMRVelocity
// {
//     double vx;
//     double vy;
//     double omega;
//     AMRVelocity()
//     {
//         vx = 0.0f;
//         vy = 0.0f;
//         omega = 0.0f;
//     };
// };

struct AMRVelocity
{
    float speed;
    float speed_toward; // speed toward
    float omega; // angluar speed
    eMoveMode mode;
    AMRVelocity()
    {
        speed = 0.0f;
        speed_toward = 0.0f;
        omega = 0.0f;
        mode = eMoveMode::STILL;
    }
};

typedef struct AMRVelocity ControlVelocity;
typedef struct AMRVelocity VehicleVelocity;